#include "Arduino.h"
#include <TinyGPS.h>
#include "UbloxGPS.h"


UbloxGPS::UbloxGPS(void){
}

void UbloxGPS::begin(Stream &port) {
  thePort = &port;
  timeOut = 2000;
  validTime = 5000;
  validChar = 1000;
  charTime = 0;
  avlTime = 0;
  status = 0;
  startAck = 0;
  gps_time = 0;
  ack1 = 1;
  ack2 = 2;
  ack3 = 3;
  nmeaClass = 0xF0;
  ggaID = 0x00;
  gllID = 0x01;
  gsaID = 0x02;
  gsvID = 0x03;
  rmcID = 0x04;
  vtgID = 0x05;
}

boolean UbloxGPS::process(void) {
  boolean avl = false;

  long t = millis();
  if ((t - charTime) < validChar) {
    status |= 0x40;
  } else {
    status &= 0xBF;
  }
  if ((t - avlTime) < validTime) {
    status |= 0x80;
  } else {
    status &= 0x7F;
  }


  while (thePort->available()) {
    charTime = t;
    char inChar = thePort->read();
    avl = avl || encode(inChar);
    //delay(1);   // May be needed for SoftwareSerial
  }
  
  if (avl) {
    avlTime = t;
    unsigned long age;
    get_position(&lat, &lon, &age);
    gps_time = age + millis();
    alt = altitude();
    /*
    g_course = int(course() / 10L);
    g_speed = int(speed() * 463L / 900L);
    cos_course = fp_cos(g_course);
    sin_course = fp_sin(g_course);
    v_north = int((long(speed) * long(cos_course)) >> 15);
    v_east = int((long(speed) * long(sin_course)) >> 15);
    */
  }
  return avl;
}

unsigned long UbloxGPS::gps_age(void) {
  unsigned long age = millis() + gps_time;
  return age;
}

boolean UbloxGPS::gps_valid(void) {
  return ((millis() - gps_time) < validTime);
}

void UbloxGPS::send_byte(byte b) {
  thePort->write(b);
  ckA += b;
  ckB += ckA;
}

void UbloxGPS::send_byte(byte b[], int length) {
  thePort->write(b, length);
  for (int i = 0; i < length; i++) {
    ckA += b[i];
    ckB += ckA;
  }
}

boolean UbloxGPS::setMode(byte mode) {
  byte mode_msg1[] = {0x06,0x24,0x24,0x00,0xFF,0xFF};
  byte mode_msg2[] = {0x03,0x00,0x00,0x00,0x00,0x10,0x27,0x00,
                      0x00,0x05,0x00,0xFA,0x00,0xFA,0x00,0x64,
                      0x00,0x2C,0x01,0x00,0x3C,0x00,0x00,0x00,
                      0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
                      0x00};  
  ckA = 0;
  ckB = 0;
  send_header();
  send_byte(mode_msg1, 6);
  send_byte(mode);
  send_byte(mode_msg2, 33);
  thePort->write(ckA);
  thePort->write(ckB);

  boolean didSet = get_ack();
  if (didSet) {
    status |= 0x01;
  } else {
    status &= 0xFE;
  }
  return didSet;
}

boolean UbloxGPS::setMsgRate(byte msgClass, byte msgID, byte rate[]) {
  byte rate_head[] = {0x06,0x01,0x08,0x00};
  ckA = 0;
  ckB = 0;
  send_header();
  send_byte(rate_head, 4);
  send_byte(msgClass);
  send_byte(msgID);
  send_byte(rate, 6);
  thePort->write(ckA);
  thePort->write(ckB);

  boolean didSet = get_ack();
  byte m = 0x01 << msgID;
  if (didSet) {
    status |= m;
  } else {
    status &= !m;
  }
  return didSet;
}


void UbloxGPS::send_header(void) {
  thePort->write(0xB5);
  thePort->write(0x62);
}

boolean UbloxGPS::get_ack(void) {
  long s_time = millis();
  boolean working = true;
  int mode = startAck;
  boolean success = false;
  while (working) {
    if (thePort->available()) {
      byte inChar = thePort->read();
      switch (mode) {
        case 0: {
          if (inChar == 0xB5) {
            mode = ack1;
          } else {
            mode = startAck;
          }
          break;
        }
        case 1: {
          if (inChar == 0x62) { 
            mode = ack2;
          } else {
            mode = startAck;
          }
          break;
        }
        case 2: {
          if (inChar == 0x05) {
            mode = ack3;
          } else {
            mode = startAck;
          }
          break;
        }
        case 3: {
          if (inChar == 0x01) {
            success = true;
            working = false;
            err = 0;
          } else if (inChar == 0x00) {;
            working = false;
            err = 1;
          } else {
            mode = 0;
          }
          break;
        }
        default: {
          mode = 0;
        }
      }
    }
    if ((millis() - s_time) > timeOut) {
      working = false;
      err = -1;
    }
  }
  return success;
}


/*
static int UbloxGPS::fp_sin(int deg10) {
  // computes sin * 32768 (sin << 15) for input deg10 in 10's of a degree
  const int D90 = 900;
  const int D180 = 1800;
  const int D270 = 2700;
  const int D360 = 3600;

  int pm = 1;
  if (deg10 < 0) {
    deg10 = -deg10;
    pm = -1;
  }
  if (deg10 >= D360) {
    deg10 = deg10 % D360;
  }
  
  if (deg10 >= D270) {
    deg10 = D360 - deg10;
    pm = -pm;
  } else if (deg10 >= D180) {
    deg10 = deg10 - D180;
    pm = -pm; 
  } else if (deg10 >= D90) {
    deg10 = D180 - deg10;
  }

  return pm * itrig(deg10);
}

static int UbloxGPS::fp_cos(int deg10) {
  // computes cos * 32768 (cos << 15) for input deg10 in 10's of a degree
  const int D90 = 900;
  const int D180 = 1800;
  const int D270 = 2700;
  const int D360 = 3600;

  int pm = 1;
  deg10 = abs(deg10);
  if (deg10 >= D360) {
    deg10 = deg10 % D360;
  }
  
  if (deg10 >= D270) {
    deg10 = deg10 - D270;
  } else if (deg10 >= D180) {
    deg10 = D270 - deg10;
    pm = -1; 
  } else if (deg10 >= D90) {
    deg10 = deg10 - D90;
    pm = -1;
  } else {
    deg10 = D90 - deg10;
  }

  return pm * itrig(deg10);
}
*/

/*
static int UbloxGPS::itrig(int deg10) {
  prog_int16_t SIN_TABLE_0[] PROGMEM = {    -2,  5690, 11205, 16384, 21062,
                                         25101, 28376, 30791, 32269, 32767};
  prog_int16_t SIN_TABLE_1[] PROGMEM = {230, 226, 216, 199, 176,
                                        148, 115,  79,  40,   0};
  prog_int16_t SIN_TABLE_2[] PROGMEM = { -20,  -48,  -86, -117, -143,
                                        -167, -183, -196, -205,    0};
  int SIN_SHIFT_1 = 2;
  int SIN_SHIFT_2 = 10;

  int idx = constrain(deg10 / 100, 0, 9);
  deg10 = deg10 % 100;

  int st0 = pgm_read_word_near(SIN_TABLE_0 + idx);
  int st1 = pgm_read_word_near(SIN_TABLE_1 + idx);
  int st2 = pgm_read_word_near(SIN_TABLE_2 + idx);

  int temp = (((((st2 * deg10) >> SIN_SHIFT_2) + st1) * deg10) >> SIN_SHIFT_1) + st0;
  return temp;
}
*/